from math import *
from string import *
from time import *
from yapgvb import *
from __future__ import division
from random import *
from traceback import *
from Queue import *


class Node(object):
    def __init__(self, imgID, image, n, s, e, w ):
        self.__imgID = imgID  # passed in 
        self.__distList = [n, s, e, w]
        self.__dirList = [None, None, None, None]
        self.__visList = [False, False, False, False]
        self.__image = image  # passed in
        self.__sink = False
        self.__give = False
        self.__next = ' '
        self.__prev = ' '
        self.__travelDist = -1
        self.__coord = [0, 0]
        self.__checked = False
        #print "new node: ", self.__imgID
        #print "dists: ", self.__distList
        

    def printNode(self):
        print self.__imgID
        print "North:"
        if(self.__dirList[0] != None):
            print "Node: ", self.__dirList[0].getID()
        print "Dist: ", self.__distList[0]
        print "Vis: ", self.__visList[0]
        print ""
        print "East:"
        if(self.__dirList[2] != None):
            print "Node: ", self.__dirList[2].getID()
        print "Dist: ", self.__distList[2]
        print "Vis: ", self.__visList[2]
        print ""
        print "West:"
        if(self.__dirList[3] != None):
            print "Node: ", self.__dirList[3].getID()
        print "Dist: ", self.__distList[3]
        print "Vis: ", self.__visList[3]
        print ""
        print "South:"
        if(self.__dirList[1] != None):
            print "Node: ", self.__dirList[1].getID()
        print "Dist: ", self.__distList[1]
        print "Vis: ", self.__visList[1]
        print ""
        
    def getImg(self):
        return self.__image

    def setImg(self, image):
        self.__image = image

    def getPrev(self):
        return self.__prev

    def setPrev(self, prev):
        self.__prev = prev

    def getNext(self):
        return self.__next

    def setNext(self, next):
        self.__next = next

    def setTravel(self, dist):
        self.__travelDist = dist

    def getTravel(self):
        return self.__travelDist

    def setChecked(self, checked):
        self.__checked = checked

    def getChecked(self):
        return self.__checked

    def setCoord(self, coord):
        self.__coord = coord

    def getCoord(self):
        return self.__coord

    def setID(self, imgID):
        self.__imgID = imgID

    def getID(self):
        return self.__imgID

    def setSink(self, sink):
        self.__sink = sink

    def getSink(self):
        return self.__sink

    def setGive(self, give):
        self.__give = give

    def getGive(self):
        return self.__give

    def setDir(self, idx, next, distTraveled):
        self.__dirList[idx] = next
        self.__distList[idx] = distTraveled

    def getDir(self, idx):
        return self.__dirList[idx]

    def setDist(self, idx, dist):
        self.__distList[idx] = dist

    def getDist(self, idx):
        return self.__distList[idx]

    def setVis(self, idx):
        self.__visList[idx] = True

    def getVis(self, idx):
        return self.__visList[idx]

    def setVisList(self, direction, visited):
        if direction == 'N':
            self.__visList[0] = visited
        elif direction == 'E':
            self.__visList[1] = visited
        elif direction == 'S':
            self.__visList[2] = visited
        elif direction == 'W':
            self.__visList[3] = visited

    def getVisList(self, direction):
        if direction == 'N':
            return self.__visList[0]
        elif direction == 'E':
            return self.__visList[1]
        elif direction == 'S':
            return self.__visList[2]
        elif direction == 'W':
            return self.__visList[3]


class Map(object):
    def __init__(self, n, s, e, w, imgID, image):
        self.__home = Node(imgID, image, n, s, e, w)
        self.__currentNode = self.__home
        self.__nodeList = [self.__home]
        self.__currentDir = 'N'
        self.__travelList = []
        #travelnode struct data
        self.__travel = None

    def getCurrDir(self):
        return self.__currentDir

    def turnDir(self, dir):
        if(self.__currentDir == 'N'):
            if(dir == 'N'):
                return 'F'
            elif(dir == 'S'):
                return 'B'
            elif(dir == 'E'):
                return 'R'
            elif(dir == 'W'):
                return 'L'
            else:
                return 'X'
        elif(self.__currentDir == 'S'):
            if(dir == 'N'):
                return 'B'
            elif(dir == 'S'):
                return 'F'
            elif(dir == 'E'):
                return 'L'
            elif(dir == 'W'):
                return 'R'
            else:
                return 'X'
        elif(self.__currentDir == 'E'):
            if(dir == 'N'):
                return 'L'
            elif(dir == 'S'):
                return 'R'
            elif(dir == 'E'):
                return 'F'
            elif(dir == 'W'):
                return 'B'
            else:
                return 'X'
        elif(self.__currentDir == 'W'):
            if(dir == 'N'):
                return 'R'
            elif(dir == 'S'):
                return 'L'
            elif(dir == 'E'):
                return 'B'
            elif(dir == 'W'):
                return 'F'
            else:
                return 'X'
        else:
            return 'X'

    def oppDir(self, dir):
        if(dir == "N"):
            return "S"
        elif(dir == "S"):
            return "N"
        elif(dir == "E"):
            return "W"
        elif(dir == "W"): 
            return "E"
        else:
            return "X"

    def oppI(self, dir):
        return self.dirtoi(self.oppDir(self.itodir(dir)))

    def turn(self, dir):
        self.__currentDir = self.changeDir(dir)
        self.__currentNode.setVis(self.dirtoi(self.__currentDir))

    def changeDir(self, dir):
        if(dir == "R"):
            #print "Turning Right"
            if(self.__currentDir == "N"):
                return "E"
            elif(self.__currentDir == "S"):
                return "W"
            elif(self.__currentDir == "E"):
                return "S"
            elif(self.__currentDir == "W"):
                return "N"
            else:
                return "X"
        elif(dir =="L"):
            #print "Turning Left"
            if(self.__currentDir == "N"):
                 return "W"
            elif(self.__currentDir == "S"):
                return "E"
            elif(self.__currentDir == "E"):
                return "N"
            elif(self.__currentDir== "W"):
                return "S"
            else:
                return "X"
        else:
            return "X"
            
    def dirtoi(self, dir):
        if(dir == "N"):
            return 0
        elif(dir == "S"):
            return 1
        elif(dir == "E"):
            return 2
        elif(dir== "W"):
            return 3
        else:
            return -1

    def itodir(self, dir):
        if(dir == 0):
            return "N"
        elif(dir == 1):
            return "S"
        elif(dir == 2):
            return "E"
        elif(dir== 3):
            return "W"
        else:
            return "X"       
    
    #add node, f, b, r, l are directions around the robot
    #pass in -1 if cannot go that direction, 0 if can go but
    #don't know the distance
    def addNode(self, f, b, r, l, imgID, image, distTraveled, sink, give):

        coord = []

        #if currently going North
        if(self.__currentDir == "N"):
            #check if same as another node
            coord = self.__currentNode.getCoord()
            check = self.same(coord[0],coord[1] + distTraveled)
      
            if(check == None):
                next = Node(imgID, image, f, b, r, l)
                next.setCoord([coord[0],coord[1] + distTraveled])
                self.__nodeList.append(next)
            else:
                next = check;

        #if currently going South
        elif(self.__currentDir == "S"):
            coord = self.__currentNode.getCoord()
            check = self.same(coord[0],coord[1] - distTraveled)

            if(check == None):
                next = Node(imgID, image, b, f, l, r)
                next.setCoord([coord[0],coord[1] - distTraveled])
                self.__nodeList.append(next)
            else:
                next = check;

        #if currently going East
        elif(self.__currentDir == "E"):
            coord = self.__currentNode.getCoord()
            check = self.same(coord[0] + distTraveled,coord[1])

            if(check == None):
                next = Node(imgID, image, l, r, f, b)
                next.setCoord([coord[0] + distTraveled,coord[1]])
                self.__nodeList.append(next)
            else:
                next = check;

        #if currently going West
        elif(self.__currentDir == "W"):
            coord = self.__currentNode.getCoord()
            check = self.same(coord[0] - distTraveled,coord[1])

            if(check == None):
                next = Node(imgID, image, r, l, b, f)
                next.setCoord([coord[0] - distTraveled,coord[1]])
                self.__nodeList.append(next)
            else:
                next = check;
        else:
            return False

        #setup the paths to and from the new node
        print "New Node"
        next.setDir(self.dirtoi(self.oppDir(self.__currentDir)), self.__currentNode, distTraveled)
        next.setGive(give)
        next.setSink(sink)
        
        self.__currentNode.setDir(self.dirtoi(self.__currentDir), next, distTraveled)

        self.__currentNode = next
        print "At: ", self.__currentNode.getID()
##        self.__travelList.append(self.__currentNode.getID())
        if(check == None):
            return True

    def whereNext(self):
        self.__travel = self.__travelList.pop()
        self.__currentNode = self.__travel["node"]
        print "Now at: ", self.__currentNode.getID()
        return self.turnDir(self.__travel["travelDir"])

    def shortestPath(self, img):
        nodes = Queue()
        to = None
        
        for node in self.__nodeList:
            node.setTravel(-1)
            #print "Checking for image"
            if(self.cmpImg(img, node.getImg())):
                #print "image found"
                win0 = GraphWin("imgin", 320, 240)
                image = Image(Point(160, 120), makePixmap(img))
                image.draw(win0)
                win1 = GraphWin("imgCheck", 320, 240)
                image = Image(Point(160, 120), makePixmap(node.getImg()))
                image.draw(win1)
                to = node
                print self.__currentNode.getID()
                print to.getID()

        if(to == None):
            return False

        if(to.getID == self.__currentNode.getID()):
            self.__travelList = []
            self.__travel = {}
            self.__travel["node"] = to
            self.__travel["isTo"] = True
            self.__travel["travelDir"] = 'X'
            self.__travelList.append(self.__travel)
            return True

        curr = self.__currentNode
        curr.setTravel(0)
        nodes.put(curr)

        while(not nodes.empty()):
            curr = nodes.get()

            for i in range(4):
                if(curr.getDist(i) > 0 and not i == self.dirtoi(node.getPrev())):
                    next = curr.getDir(i)
                    if(next.getSink() == True):
                        mod = 5
                    elif(next.getGive() == True):
                        mod = -30
                    else:
                        mod = 0
                        
                    if(next.getTravel() == -1 or curr.getTravel() + curr.getDist(i) < next.getTravel()):
                        next.setTravel(curr.getTravel() + curr.getDist(i) + 5)
                        next.setPrev(self.oppDir(self.itodir(i)))
                        if(to.getID() != next.getID()):
                            nodes.put(next)

        done = False
        self.__travelList = []
        self.__travel = {}
        self.__travel["node"] = to
        self.__travel["isTo"] = True
        self.__travel["travelDir"] = 'X'
        self.__travelList.append(self.__travel)
        next = to.getDir(self.dirtoi(to.getPrev()))
        next.setNext(self.oppDir(to.getPrev()))
        curr = next
        while(done == False):
            self.__travel = {}
            self.__travel["node"] = curr
            self.__travel["travelDir"] = curr.getNext()
            self.__travel["isTo"] = False
            self.__travelList.append(self.__travel)

            if(curr.getID() == self.__currentNode.getID()):
                done = True
            else:
                next = curr.getDir(self.dirtoi(curr.getPrev()))
                next.setNext(self.oppDir(curr.getPrev()))
                curr = next

        return True
        
    def cmpImg(self, img1, img2):
        width1 = getWidth(img1)
        height1 = getHeight(img1)
        width2 = getWidth(img2)
        height2 = getHeight(img2)

        if(not width1 == width2) or (not height1 == height2):
            return False
        else:
            x = 0
            y = 0
            for y in range(0, height1, 5):
                for x in range(0, width1, 5):
                    r1, g1, b1 = getRGB(getPixel(img1, x, y))
                    r2, g2, b2 = getRGB(getPixel(img2, x, y))
                    #print str(r1) + " " + str(g1) + " " + str(b1)
                    #print str(r2) + " " + str(g2) + " " + str(b2)
                    if(r1 != r2 or g1 != g2 or b1 != b2):
                        #print "unequal"
                        return False
            return True

    def same(self, x,y):
        for i in range(len(self.__nodeList)):
            coord = self.__nodeList[i].getCoord()
            if(coord[0] == x and coord[1] == y):
                return self.__nodeList[i]
        return None

    def printGraph(self):
        graph = GraphWin("Graph", 800, 800)
        graph.setCoords(-50, -50, 80, 80)
        white = color_rgb(0, 0, 0)
        black = color_rgb(255, 255, 255)
        graph.setBackground(white)
        for node in self.__nodeList:
            coord = node.getCoord()
            imgID = node.getID()
            #print imgID
            #print coord
            coord[0] = (coord[0] * 5)
            coord[1] = (coord[1] * 5)
            #print coord
            txt = Text(Point(coord[0], coord[1]), imgID)
            txt.setTextColor(black)
            txt.draw(graph)
            if(node.getDist(0) > 0):
                newCoord = coord[:]
                travDist = node.getDist(0)
                for i in range(travDist - 1):
                    newCoord[1] += 5
                    txt = Text(Point(newCoord[0], newCoord[1]), "0")
                    txt.setTextColor(black)
                    txt.draw(graph)
            if(node.getDist(1) > 0):
                newCoord = coord[:]
                travDist = node.getDist(1)
                for i in range(travDist - 1):
                    newCoord[1] -= 5
                    txt = Text(Point(newCoord[0], newCoord[1]), "0")
                    txt.setTextColor(black)
                    txt.draw(graph)
            if(node.getDist(2) > 0):
                newCoord = coord[:]
                travDist = node.getDist(2)
                for i in range(travDist - 1):
                    newCoord[0] += 5
                    txt = Text(Point(newCoord[0], newCoord[1]), "0")
                    txt.setTextColor(black)
                    txt.draw(graph)
            if(node.getDist(3) > 0):
                newCoord = coord[:]
                travDist = node.getDist(3)
                for i in range(travDist - 1):
                    newCoord[0] -= 5
                    txt = Text(Point(newCoord[0], newCoord[1]), "0")
                    txt.setTextColor(black)
                    txt.draw(graph)
        
    def amtUnvis(self):
        amt = 0
        for node in self.__nodeList:
            for i in range(4):
                if(node.getDist(i) == 0):
                    amt += 1
        return amt

    def checkNext(self):
        seed()
        unvisitedAmt = [-1, -1, -1, -1]
        unvisited = 0
        weight = 1
        paths = 0
        posb = []

        #print "Amt Unvisited: ", self.amtUnvis()
        if(self.amtUnvis() == 0):
            return 'X'

        for i in range(4):
            if(self.__currentNode.getDist(i) == -1):
                paths += 1
        if(paths == 3):
            return 'B'

        for i in range(4):
            if(self.__currentNode.getDist(i) == 0):
                return self.turnDir(self.itodir(i))
##                posb.append(self.turnDir(self.itodir(i)))
##        if(len(posb) > 0):
##            dir = randint(0, len(posb) - 1)
##            return posb[dir]

        for i in range(4):
            if(self.__currentNode.getDist(i) > 0 and self.__currentNode.getDir(i) != None):
                for node in self.__nodeList:
                    node.setChecked(False)
                self.__currentNode.setChecked(True)
                unvisitedAmt[i] = self.recurseNext(self.__currentNode.getDir(i), weight + 1, i)
            if(unvisitedAmt[i] > unvisitedAmt[unvisited]):
                unvisited = i

        #print "N, S, E, W:", unvisitedAmt
        return self.turnDir(self.itodir(unvisited))

    def recurseNext(self, node, weight, source):
        unVisCount = 0
        node.setChecked(True)
##        iterS = ""
##        for i in range(weight):
##            iterS += "  "
##        #print iterS + "Source: ", self.itodir(source)
        for i in range(4):
            if(node.getDist(i) == 0 and i != self.oppI(source)):
##                #print iterS + "Path Weight: ", 1/weight
                unVisCount += (1/weight)
            if(node.getDist(i) > 0 and i != self.oppI(source) and node.getDir(i) != None and node.getDir(i).getChecked() == False):
##                #print iterS + "Check: " + str(self.itodir(i)) + " Node: ", node.getDir(i).getID() 
                newUnvis = self.recurseNext(node.getDir(i), weight+1, i)
##                #print iterS + "New Unvis: ", newUnvis
                unVisCount += newUnvis

        return unVisCount

    def printMap(self):
        for node in self.__nodeList:
            node.printNode()

    def printList(self):
        for node in self.__travelList:
            print node

def run():
    try:
        loc, gaze, ergs = initSim()
        f, b, r, l = getPaths(gaze)
        myMap = Map(f, b, r, l, "Home", loc)
        places = 1
        next = myMap.checkNext()
        dist = 0

        while(next != 'X'):
            turnTo(next, myMap)
            loc, gaze, newErgs = go(1)
            dist += 1
            #print myMap.getCurrDir()
            #print "Picture is Blank: ", isBlank(loc)
            #only ad node if it is an id node
            if(isBlank(loc) == False):
                
                f, b, r, l = getPaths(gaze)
                spent = ergs - newErgs

                if(spent > 1):
                    sink = True
                    #print "Sink"
                else:
                    sink = False
                if(spent < 0):
                    give = True
                    #print "give"
                else:
                    give = False

                ergs = newErgs

                if(give == True and ergs < 70):
                    #print "############################################ Filling Up"
                    while(newErgs < 70):
                        #print "################################################ ", newErgs
                        turnTo('B', myMap)
                        loc, gaze, newErgs = go(1)
                        turnTo('B', myMap)
                        loc, gaze, newErgs = go(1)
                    ergs = newErgs
                    #print "############################################ Done Filling"

                success = myMap.addNode(f, b, r, l, str(places), loc, dist, sink, give)
                if(success == True):
                    places += 1
                next = myMap.checkNext()
                #print next
                dist = 0
            else:
                next = 'F'
        myMap.printGraph()
        imgName = pickAFile()
        dist = 0
        while(imgName != ""):
            print imgName
            img = makePicture(imgName)
            if(myMap.shortestPath(img)):
                myMap.printList()

                next = myMap.whereNext()
                while(next != 'X'):
                    turnTo(next, myMap)
                    loc, gaze, ergs = go(1)
                    dist += 1
                    print ergs
                    if(isBlank(loc) == False):
                        next = myMap.whereNext()
                        dist = 0
                    else:
                        next = 'F'
            imgName = pickAFile()

        
    except Exception, e:
        print_exc()
        myMap.printGraph()
        #myMap.printMap()
        #myMap.printList()

def turnTo(next, myMap):
    if(next == 'B'):
        #print "*********Turning Around************"
        R90()
        R90()
        myMap.turn('R')
        myMap.turn('R')
    elif(next == 'R'):
        R90()
        myMap.turn('R')
    elif(next == 'L'):
        L90()
        myMap.turn('L')

def isBlank(img):
    r, g, b = 255, 255, 255
    width = getWidth(img)
    height = getHeight(img)
    
    for row in range(0, height, 10):
        for col in range(0, width, 10):
            newR, newG, newB = getRGB(getPixel(img, col, row))
            if(not r == newR or not g == newG or not b == newB):
                return False
    return True

def getPaths(gaze):
    paths = [0, 0, 0, 0]
    
    for i in range(4):
        #print i
        #show(gaze)
        #sleep(3)
        if(isWall(gaze)):
            paths[i] = -1
        else:
            paths[i] = 0
        gaze = R90()

    #print paths[0], paths[2], paths[1], paths[3]


    return paths[0], paths[2], paths[1], paths[3]
